% TITLE: Individual Anlysis- Hand Movement Simulation
% By: Jannell Broderick
% Course: ME 486C
% DATE: 02/28/2019
% Goal: This code will successfully plot the movement of a fingers over
    % time. The postion and velocity of the fingers will be
    % found. This includes the finger ends and center of masses of each
    % segment of the finger (Proximal and Distal).

    % The fingers are moved via artificial tendons that will flex and
    % relax the finger. 
    
    % The fingers 2-4 should all have similar movement. However, digit 1
    % (the thumb) will have roational movement in addition to the
    % flexation. (The thumb will be more challenging). 
    

%% House Keeping
clc;            % this will clear the fx >> command window
clear;          % this clears local variables so there is no conflicts
close all;      % close plots so each plot windows is new and correct
format short;   % only show 5 significant digits for clear outputs

%% Inputs
%Digit 1 (thumb)
B_1 = 0.25;%(in) %Length Base
Theta_B_1 = pi;%(rad) %Angle of Base with respect to X 
%Note: Not a hinge so always pi
P_1 = 0.75;%(in) %Length of Proximal Segment
k = 4;
for T_P_1 = 0:pi/(2*k):pi/3; T_M_1 = 0:pi/(2*k):pi/3; 
for T_P_2 = 0:pi/(2*k):pi/3; T_M_2 = 0:pi/(2*k):pi/3;
for T_P_3 = 0:pi/(2*k):pi/3; T_M_3 = 0:pi/(2*k):pi/3;
for T_P_4 = 0:pi/(2*k):pi/3; T_M_4 = 0:pi/(2*k):pi/3;
for T_P_5 = 0:pi/(2*k):pi/3; T_M_5 = 0:pi/(2*k):pi/3;

% T_P_1 = 0;%(rad) %Angle of Proximal
M_1 = 0.5;%(in) %Length of Middle Segment
% T_M_1 = 0;%(rad) %Angle of Middle
D_1 = 0.25;%(in) %Length of Distal Segment

    Theta_P_1 = T_P_1 + Theta_B_1;%(rad) %Angle of Proximal with respect to X
    Theta_M_1 = T_M_1 + Theta_P_1;%(rad) %Angle of Middle with respect to X
    Theta_D_1 = pi/12 + Theta_M_1;%(rad) %Angle of Distal with respect to X 
    %Note: Dital and Middle segments are one part

%Digit 2 (Pointer
B_2 = 0.00001;%(in) %Length Base
Theta_B_2 = pi;%(rad) %Angle of Base with respect to X 
%Note: Not a hinge so always pi
P_2 = 0.65;%(in) %Length of Proximal Segment
% T_P_2 = 0;%(rad) %Angle of Proximal
M_2 = 0.4;%(in) %Length of Middle Segment
% T_M_2 = 0;%(rad) %Angle of Middle
D_2 = 0.15;%(in) %Length of Distal Segment

    Theta_P_2 = T_P_2 + Theta_B_2;%(rad) %Angle of Proximal with respect to X
    Theta_M_2 = T_M_2 + Theta_P_2;%(rad) %Angle of Middle with respect to X
    Theta_D_2 = pi/12 + Theta_M_2;%(rad) %Angle of Distal with respect to X 
    %Note: Dital and Middle segments are one part so =pi/6 + Theta_M_1

%Digit 3 (Middle)
B_3 = 0.00001;%(in) %Length Base
Theta_B_3 = pi;%(rad) %Angle of Base with respect to X 
%Note: Not a hinge so always pi
P_3 = 0.75;%(in) %Length of Proximal Segment
%T_P_3 = 0;%(rad) %Angle of Proximal
M_3 = 0.5;%(in) %Length of Middle Segment
%T_M_3 = 0;%(rad) %Angle of Middle
D_3 = 0.25;%(in) %Length of Distal Segment

    Theta_P_3 = T_P_3 + Theta_B_3;%(rad) %Angle of Proximal with respect to X
    Theta_M_3 = T_M_3 + Theta_P_3;%(rad) %Angle of Middle with respect to X
    Theta_D_3 = pi/12 + Theta_M_3;%(rad) %Angle of Distal with respect to X 
    %Note: Dital and Middle segments are one part so =pi/6 + Theta_M_1

%Digit 4 (Ring)
B_4 = 0.00001;%(in) %Length Base
Theta_B_4 = pi;%(rad) %Angle of Base with respect to X 
%Note: Not a hinge so always pi
P_4 = 0.65;%(in) %Length of Proximal Segment
%T_P_4 = 0;%(rad) %Angle of Proximal
M_4 = 0.4;%(in) %Length of Middle Segment
%T_M_4 = 0;%(rad) %Angle of Middle
D_4 = 0.15;%(in) %Length of Distal Segment

    Theta_P_4 = T_P_4 + Theta_B_4;%(rad) %Angle of Proximal with respect to X
    Theta_M_4 = T_M_4 + Theta_P_4;%(rad) %Angle of Middle with respect to X
    Theta_D_4 = pi/12 + Theta_M_4;%(rad) %Angle of Distal with respect to X 
    %Note: Dital and Middle segments are one part so =pi/6 + Theta_M_1

%Digit 5 (Pinky)
B_5 = 0.00001;%(in) %Length Base
Theta_B_5 = pi;%(rad) %Angle of Base with respect to X 
%Note: Not a hinge so always pi
P_5 = 0.55;%(in) %Length of Proximal Segment
%T_P_5 = 0;%(rad) %Angle of Proximal
M_5 = 0.3;%(in) %Length of Middle Segment
%T_M_5 = 0;%(rad) %Angle of Middle
D_5 = 0.10;%(in) %Length of Distal Segment

    Theta_P_5 = T_P_5 + Theta_B_5;%(rad) %Angle of Proximal with respect to X
    Theta_M_5 = T_M_5 + Theta_P_5;%(rad) %Angle of Middle with respect to X
    Theta_D_5 = pi/12 + Theta_M_5;%(rad) %Angle of Distal with respect to X 
    %Note: Dital and Middle segments are one part so =pi/6 + Theta_M_1

%% Motor Information

Motor_radius = 0.25; %(in) % Use the radius to calcualte amount of artificial tendon is being pulled
Rot_Angle = pi/6;
Arc_length = Rot_Angle*Motor_radius;

%% Digit 1 (thumb)
n=100;

B_1_X = 0:(B_1*cos(Theta_B_1))/(n-1):B_1*cos(Theta_B_1); %(in) %Posistion in X of Rotating Base
B_1_Y = 0:(B_1*sin(Theta_B_1))/(n-1):B_1*sin(Theta_B_1); %(in) %Posistion in Y of Rotating Base

P_1_X = B_1_X(1,end):(P_1*cos(Theta_P_1))/(n-1):B_1_X(1,end)+P_1*cos(Theta_P_1); %(in) %Posistion in X of Proximal Segment
P_1_Y = B_1_Y(1,end):(P_1*sin(Theta_P_1))/(n-1):B_1_Y(1,end)+P_1*sin(Theta_P_1); %(in) %Posistion in Y of Proximal Segment

M_1_X = P_1_X(1,end):(M_1*cos(Theta_M_1))/(n-1):P_1_X(1,end)+M_1*cos(Theta_M_1); %(in) %Posistion in X of Distal Segment
M_1_Y = P_1_Y(1,end):(M_1*sin(Theta_M_1))/(n-1):P_1_Y(1,end)+M_1*sin(Theta_M_1); %(in) %Posistion in Y of Distal Segment

D_1_X = M_1_X(1,end):(D_1*cos(Theta_D_1))/(n-1):M_1_X(1,end)+D_1*cos(Theta_D_1); %(in) %Posistion in X of Distal Segment
D_1_Y = M_1_Y(1,end):(D_1*sin(Theta_D_1))/(n-1):M_1_Y(1,end)+D_1*sin(Theta_D_1); %(in) %Posistion in Y of Distal Segment

%Connecting finger segments end to end
Length_X_1 =length(B_1_X)+ length(P_1_X)+length(M_1_X)+length(D_1_X );
X_1 = zeros(1,Length_X_1);
X_1(1:length(B_1_X))= -(B_1_X);
X_1((length(B_1_X)+1):(length(B_1_X)+length(P_1_X)))= -(P_1_X);
X_1((length(B_1_X)+length(P_1_X)+1):(length(B_1_X)+length(P_1_X)+length(M_1_X)))= -(M_1_X);
X_1((length(B_1_X)+length(P_1_X)+length(M_1_X)+1):Length_X_1)= -(D_1_X);
X_1 = X_1 -1;

Length_Y_1 =length(B_1_Y)+ length(P_1_Y)+length(M_1_Y)+length(D_1_Y);
Y_1 = zeros(1,Length_Y_1);
Y_1(1:length(B_1_Y))= B_1_Y;
Y_1((length(B_1_Y)+1):(length(B_1_Y)+length(P_1_Y)))= P_1_Y;
Y_1((length(B_1_Y)+length(P_1_Y)+1):(length(B_1_Y)+length(P_1_Y)+length(M_1_Y)))= M_1_Y;
Y_1((length(B_1_Y)+length(P_1_Y)+length(M_1_Y)+1):Length_Y_1)= D_1_Y;

Z_1 = zeros(1,length(X_1));

plot3 (X_1,Y_1,Z_1,'r') %Digit 1
xlabel ('x')
ylabel ('y')
zlabel ('z')
hold on
%% Digit 2 (Pointer)
B_2_X = 0:(B_2*cos(Theta_B_2))/(n-1):B_2*cos(Theta_B_2); %(in) %Posistion in X of Rotating Base
B_2_Y = 0:(B_2*sin(Theta_B_2))/(n-1):B_2*sin(Theta_B_2); %(in) %Posistion in Y of Rotating Base

P_2_X = B_2_X(1,end):(P_2*cos(Theta_P_2))/(n-1):B_2_X(1,end)+P_2*cos(Theta_P_2); %(in) %Posistion in X of Proximal Segment
P_2_Y = B_2_Y(1,end):(P_2*sin(Theta_P_2))/(n-1):B_2_Y(1,end)+P_2*sin(Theta_P_2); %(in) %Posistion in Y of Proximal Segment

M_2_X = P_2_X(1,end):(M_2*cos(Theta_M_2))/(n-1):P_2_X(1,end)+M_2*cos(Theta_M_2); %(in) %Posistion in X of Middle Segment
M_2_Y = P_2_Y(1,end):(M_2*sin(Theta_M_2))/(n-1):P_2_Y(1,end)+M_2*sin(Theta_M_2); %(in) %Posistion in Y of Middle Segment

D_2_X = M_2_X(1,end):(D_2*cos(Theta_D_2))/(n-1):M_2_X(1,end)+D_2*cos(Theta_D_2); %(in) %Posistion in X of Distal Segment
D_2_Y = M_2_Y(1,end):(D_2*sin(Theta_D_2))/(n-1):M_2_Y(1,end)+D_2*sin(Theta_D_2); %(in) %Posistion in Y of Distal Segment

%Connecting finger segments end to end
Length_X_2 =length(B_2_X)+ length(P_2_X)+length(M_2_X)+length(D_2_X );
X_2 = zeros(1,Length_X_2);
X_2(1:length(B_2_X))= -(B_2_X);
X_2((length(B_2_X)+1):(length(B_2_X)+length(P_2_X)))= -(P_2_X);
X_2((length(B_2_X)+length(P_2_X)+1):(length(B_2_X)+length(P_2_X)+length(M_2_X)))= -(M_2_X);
X_2((length(B_2_X)+length(P_2_X)+length(M_2_X)+1):Length_X_2)= -(D_2_X);

Length_Y_2 =length(B_2_Y)+ length(P_2_Y)+length(M_2_Y)+length(D_2_Y);
Y_2 = zeros(1,Length_Y_2);
Y_2(1:length(B_2_Y))= B_2_Y;
Y_2((length(B_2_Y)+1):(length(B_2_Y)+length(P_2_Y)))= P_2_Y;
Y_2((length(B_2_Y)+length(P_2_Y)+1):(length(B_2_Y)+length(P_2_Y)+length(M_2_Y)))= M_2_Y;
Y_2((length(B_2_Y)+length(P_2_Y)+length(M_2_Y)+1):Length_Y_2)= D_2_Y;

Z_2 = -ones(1,length(X_2));

plot3 (X_2,Y_2,Z_2,'b') %Digit 2
xlabel ('x')
ylabel ('y')
zlabel ('z')
hold on
 
%% Digit 3 (Middle)
B_3_X = 0:(B_3*cos(Theta_B_3))/(n-1):B_3*cos(Theta_B_3); %(in) %Posistion in X of Rotating Base
B_3_Y = 0:(B_3*sin(Theta_B_3))/(n-1):B_3*sin(Theta_B_3); %(in) %Posistion in Y of Rotating Base

P_3_X = B_3_X(1,end):(P_3*cos(Theta_P_3))/(n-1):B_3_X(1,end)+P_3*cos(Theta_P_3); %(in) %Posistion in X of Proximal Segment
P_3_Y = B_3_Y(1,end):(P_3*sin(Theta_P_3))/(n-1):B_3_Y(1,end)+P_3*sin(Theta_P_3); %(in) %Posistion in Y of Proximal Segment

M_3_X = P_3_X(1,end):(M_3*cos(Theta_M_3))/(n-1):P_3_X(1,end)+M_3*cos(Theta_M_3); %(in) %Posistion in X of Middle Segment
M_3_Y = P_3_Y(1,end):(M_3*sin(Theta_M_3))/(n-1):P_3_Y(1,end)+M_3*sin(Theta_M_3); %(in) %Posistion in Y of Middle Segment

D_3_X = M_3_X(1,end):(D_3*cos(Theta_D_3))/(n-1):M_3_X(1,end)+D_3*cos(Theta_D_3); %(in) %Posistion in X of Distal Segment
D_3_Y = M_3_Y(1,end):(D_3*sin(Theta_D_3))/(n-1):M_3_Y(1,end)+D_3*sin(Theta_D_3); %(in) %Posistion in Y of Distal Segment

%Connecting finger segments end to end
Length_X_3 =length(B_3_X)+ length(P_3_X)+length(M_3_X)+length(D_3_X );
X_3 = zeros(1,Length_X_3);
X_3(1:length(B_3_X))= -(B_3_X);
X_3((length(B_3_X)+1):(length(B_3_X)+length(P_3_X)))= -(P_3_X);
X_3((length(B_3_X)+length(P_3_X)+1):(length(B_3_X)+length(P_3_X)+length(M_3_X)))= -(M_3_X);
X_3((length(B_3_X)+length(P_3_X)+length(M_3_X)+1):Length_X_3)= -(D_3_X);

Length_Y_3 =length(B_3_Y)+ length(P_3_Y)+length(M_3_Y)+length(D_3_Y);
Y_3 = zeros(1,Length_Y_3);
Y_3(1:length(B_3_Y))= B_3_Y;
Y_3((length(B_3_Y)+1):(length(B_3_Y)+length(P_3_Y)))= P_3_Y;
Y_3((length(B_3_Y)+length(P_3_Y)+1):(length(B_3_Y)+length(P_3_Y)+length(M_3_Y)))= M_3_Y;
Y_3((length(B_3_Y)+length(P_3_Y)+length(M_3_Y)+1):Length_Y_3)= D_3_Y;

Z_3 = -2*ones(1,length(X_3));

plot3 (X_3,Y_3,Z_3,'g') %Digit 3
xlabel ('x')
ylabel ('y')
zlabel ('z')
hold on

%% Digit 4 (Ring)
B_4_X = 0:(B_4*cos(Theta_B_4))/(n-1):B_4*cos(Theta_B_4); %(in) %Posistion in X of Rotating Base
B_4_Y = 0:(B_4*sin(Theta_B_4))/(n-1):B_4*sin(Theta_B_4); %(in) %Posistion in Y of Rotating Base

P_4_X = B_4_X(1,end):(P_4*cos(Theta_P_4))/(n-1):B_4_X(1,end)+P_4*cos(Theta_P_4); %(in) %Posistion in X of Proximal Segment
P_4_Y = B_4_Y(1,end):(P_4*sin(Theta_P_4))/(n-1):B_4_Y(1,end)+P_4*sin(Theta_P_4); %(in) %Posistion in Y of Proximal Segment

M_4_X = P_4_X(1,end):(M_4*cos(Theta_M_4))/(n-1):P_4_X(1,end)+M_4*cos(Theta_M_4); %(in) %Posistion in X of Middle Segment
M_4_Y = P_4_Y(1,end):(M_4*sin(Theta_M_4))/(n-1):P_4_Y(1,end)+M_4*sin(Theta_M_4); %(in) %Posistion in Y of Middle Segment

D_4_X = M_4_X(1,end):(D_4*cos(Theta_D_4))/(n-1):M_4_X(1,end)+D_4*cos(Theta_D_4); %(in) %Posistion in X of Distal Segment
D_4_Y = M_4_Y(1,end):(D_4*sin(Theta_D_4))/(n-1):M_4_Y(1,end)+D_4*sin(Theta_D_4); %(in) %Posistion in Y of Distal Segment

%Connecting finger segments end to end
Length_X_4 =length(B_4_X)+ length(P_4_X)+length(M_4_X)+length(D_4_X );
X_4 = zeros(1,Length_X_4);
X_4(1:length(B_4_X))= -(B_4_X);
X_4((length(B_4_X)+1):(length(B_4_X)+length(P_4_X)))= -(P_4_X);
X_4((length(B_4_X)+length(P_4_X)+1):(length(B_4_X)+length(P_4_X)+length(M_4_X)))= -(M_4_X);
X_4((length(B_4_X)+length(P_4_X)+length(M_4_X)+1):Length_X_4)= -(D_4_X);

Length_Y_4 =length(B_4_Y)+ length(P_4_Y)+length(M_4_Y)+length(D_4_Y);
Y_4 = zeros(1,Length_Y_4);
Y_4(1:length(B_4_Y))= B_4_Y;
Y_4((length(B_4_Y)+1):(length(B_4_Y)+length(P_4_Y)))= P_4_Y;
Y_4((length(B_4_Y)+length(P_4_Y)+1):(length(B_4_Y)+length(P_4_Y)+length(M_4_Y)))= M_4_Y;
Y_4((length(B_4_Y)+length(P_4_Y)+length(M_4_Y)+1):Length_Y_4)= D_4_Y;

Z_4 = -3*ones(1,length(X_4));

plot3 (X_4,Y_4,Z_4,'c') %Digit 4
xlabel ('x')
ylabel ('y')
zlabel ('z')
hold on

%% Digit 5 (Pinky)
B_5_X = 0:(B_5*cos(Theta_B_5))/(n-1):B_5*cos(Theta_B_5); %(in) %Posistion in X of Rotating Base
B_5_Y = 0:(B_5*sin(Theta_B_5))/(n-1):B_5*sin(Theta_B_5); %(in) %Posistion in Y of Rotating Base

P_5_X = B_5_X(1,end):(P_5*cos(Theta_P_5))/(n-1):B_5_X(1,end)+P_5*cos(Theta_P_5); %(in) %Posistion in X of Proximal Segment
P_5_Y = B_5_Y(1,end):(P_5*sin(Theta_P_5))/(n-1):B_5_Y(1,end)+P_5*sin(Theta_P_5); %(in) %Posistion in Y of Proximal Segment

M_5_X = P_5_X(1,end):(M_5*cos(Theta_M_5))/(n-1):P_5_X(1,end)+M_5*cos(Theta_M_5); %(in) %Posistion in X of Middle Segment
M_5_Y = P_5_Y(1,end):(M_5*sin(Theta_M_5))/(n-1):P_5_Y(1,end)+M_5*sin(Theta_M_5); %(in) %Posistion in Y of Middle Segment

D_5_X = M_5_X(1,end):(D_5*cos(Theta_D_5))/(n-1):M_5_X(1,end)+D_5*cos(Theta_D_5); %(in) %Posistion in X of Distal Segment
D_5_Y = M_5_Y(1,end):(D_5*sin(Theta_D_5))/(n-1):M_5_Y(1,end)+D_5*sin(Theta_D_5); %(in) %Posistion in Y of Distal Segment

%Connecting finger segments end to end
Length_X_5 =length(B_5_X)+ length(P_5_X)+length(M_5_X)+length(D_5_X );
X_5 = zeros(1,Length_X_5);
X_5(1:length(B_5_X))= -(B_5_X);
X_5((length(B_5_X)+1):(length(B_5_X)+length(P_5_X)))= -(P_5_X);
X_5((length(B_5_X)+length(P_5_X)+1):(length(B_5_X)+length(P_5_X)+length(M_5_X)))= -(M_5_X);
X_5((length(B_5_X)+length(P_5_X)+length(M_5_X)+1):Length_X_5)= -(D_5_X);

Length_Y_5 =length(B_5_Y)+ length(P_5_Y)+length(M_5_Y)+length(D_5_Y);
Y_5 = zeros(1,Length_Y_5);
Y_5(1:length(B_5_Y))= B_5_Y;
Y_5((length(B_5_Y)+1):(length(B_5_Y)+length(P_5_Y)))= P_5_Y;
Y_5((length(B_5_Y)+length(P_5_Y)+1):(length(B_5_Y)+length(P_5_Y)+length(M_5_Y)))= M_5_Y;
Y_5((length(B_5_Y)+length(P_5_Y)+length(M_5_Y)+1):Length_Y_5)= D_5_Y;

Z_5 = -4*ones(1,length(X_5));

plot3 (X_5,Y_5,Z_5,'y') %Digit 5
xlabel ('x')
ylabel ('y')
zlabel ('z')
hold on

%% Positions of Ends and COMs of each segment
    % for COM I am assuming that each segment is a rectangle. So, COM is in
    % the center of each repective segment
%Digit 1 (thumb)
B_COM_1_X = -median(B_1_X);%Base %COM in X
B_COM_1_Y = median(B_1_Y);%Base %COM in Y

P_COM_1_X = -median(P_1_X);%Proximal %COM in X
P_COM_1_Y = median(P_1_Y);%Proximal %COM in Y

M_COM_1_X = -median(M_1_X);%Middle %COM in X
M_COM_1_Y = median(M_1_Y);%Middle %COM in Y

D_COM_1_X = -median(D_1_X);%Distal %COM in X
D_COM_1_Y = median(D_1_Y);%Distal %COM in Y

COM_1_X = [B_COM_1_X-1,P_COM_1_X-1,M_COM_1_X-1,D_COM_1_X-1];
COM_1_Y = [B_COM_1_Y,P_COM_1_Y,M_COM_1_Y,D_COM_1_Y];
COM_1_Z = [0,0,0,0];

plot3(COM_1_X,COM_1_Y,COM_1_Z,'*r')

END_1_X = -[B_1_X(end)+1,P_1_X(end)+1,M_1_X(end)+1,D_1_X(end)+1];
END_1_Y = [B_1_Y(end),P_1_Y(end),M_1_Y(end),D_1_Y(end)];
END_1_Z = [0,0,0,0];

plot3(END_1_X,END_1_Y,END_1_Z,'or')

%Digit 2 (Pointer)
B_COM_2_X = -median(B_2_X);%Base %COM in X
B_COM_2_Y = median(B_2_Y);%Base %COM in Y

P_COM_2_X = -median(P_2_X);%Proximal %COM in X
P_COM_2_Y = median(P_2_Y);%Proximal %COM in Y

M_COM_2_X = -median(M_2_X);%Middle %COM in X
M_COM_2_Y = median(M_2_Y);%Middle %COM in Y

D_COM_2_X = -median(D_2_X);%Distal %COM in X
D_COM_2_Y = median(D_2_Y);%Distal %COM in Y

COM_2_X = [B_COM_2_X,P_COM_2_X,M_COM_2_X,D_COM_2_X];
COM_2_Y = [B_COM_2_Y,P_COM_2_Y,M_COM_2_Y,D_COM_2_Y];
COM_2_Z = -[1,1,1,1];
plot3(COM_2_X,COM_2_Y,COM_2_Z,'*b')

END_2_X = -[B_2_X(end),P_2_X(end),M_2_X(end),D_2_X(end)];
END_2_Y = [B_2_Y(end),P_2_Y(end),M_2_Y(end),D_2_Y(end)];
END_2_Z = -1*[1,1,1,1];
plot3(END_2_X,END_2_Y,END_2_Z,'ob')

%Digit 3 (Middle)
B_COM_3_X = -median(B_3_X);%Base %COM in X
B_COM_3_Y = median(B_3_Y);%Base %COM in Y

P_COM_3_X = -median(P_3_X);%Proximal %COM in X
P_COM_3_Y = median(P_3_Y);%Proximal %COM in Y

M_COM_3_X = -median(M_3_X);%Middle %COM in X
M_COM_3_Y = median(M_3_Y);%Middle %COM in Y

D_COM_3_X = -median(D_3_X);%Distal %COM in X
D_COM_3_Y = median(D_3_Y);%Distal %COM in Y

COM_3_X = [B_COM_3_X,P_COM_3_X,M_COM_3_X,D_COM_3_X];
COM_3_Y = [B_COM_3_Y,P_COM_3_Y,M_COM_3_Y,D_COM_3_Y];
COM_3_Z = -2*[1,1,1,1];
plot3(COM_3_X,COM_3_Y,COM_3_Z,'*g')

END_3_X = -[B_3_X(end),P_3_X(end),M_3_X(end),D_3_X(end)];
END_3_Y = [B_3_Y(end),P_3_Y(end),M_3_Y(end),D_3_Y(end)];
END_3_Z = -2*[1,1,1,1];
plot3(END_3_X,END_3_Y,END_3_Z,'og')

%Digit 4 (Ring)
B_COM_4_X = -median(B_4_X);%Base %COM in X
B_COM_4_Y = median(B_4_Y);%Base %COM in Y

P_COM_4_X = -median(P_4_X);%Proximal %COM in X
P_COM_4_Y = median(P_4_Y);%Proximal %COM in Y

M_COM_4_X = -median(M_4_X);%Middle %COM in X
M_COM_4_Y = median(M_4_Y);%Middle %COM in Y

D_COM_4_X = -median(D_4_X);%Distal %COM in X
D_COM_4_Y = median(D_4_Y);%Distal %COM in Y

COM_4_X = [B_COM_4_X,P_COM_4_X,M_COM_4_X,D_COM_4_X];
COM_4_Y = [B_COM_4_Y,P_COM_4_Y,M_COM_4_Y,D_COM_4_Y];
COM_4_Z = -3*[1,1,1,1];
plot3(COM_4_X,COM_4_Y,COM_4_Z,'*c')

END_4_X = -[B_4_X(end),P_4_X(end),M_4_X(end),D_4_X(end)];
END_4_Y = [B_4_Y(end),P_4_Y(end),M_4_Y(end),D_4_Y(end)];
END_4_Z = -3*[1,1,1,1];

plot3(END_4_X,END_4_Y,END_4_Z,'oc')
%Digit 5 (Pinky)
B_COM_5_X = -median(B_5_X);%Base %COM in X
B_COM_5_Y = median(B_5_Y);%Base %COM in Y

P_COM_5_X = -median(P_5_X);%Proximal %COM in X
P_COM_5_Y = median(P_5_Y);%Proximal %COM in Y

M_COM_5_X = -median(M_5_X);%Middle %COM in X
M_COM_5_Y = median(M_5_Y);%Middle %COM in Y

D_COM_5_X = -median(D_5_X);%Distal %COM in X
D_COM_5_Y = median(D_5_Y);%Distal %COM in Y

COM_5_X = [B_COM_5_X,P_COM_5_X,M_COM_5_X,D_COM_5_X];
COM_5_Y = [B_COM_5_Y,P_COM_5_Y,M_COM_5_Y,D_COM_5_Y];
COM_5_Z = -4*[1,1,1,1];
plot3(COM_5_X,COM_5_Y,COM_5_Z,'*y')

END_5_X = -[B_5_X(end),P_5_X(end),M_5_X(end),D_5_X(end)];
END_5_Y = [B_5_Y(end),P_5_Y(end),M_5_Y(end),D_5_Y(end)];
END_5_Z = -4*[1,1,1,1];

plot3(END_5_X,END_5_Y,END_5_Z,'oy')
legend ('Digit 1','Digit 2','Digit 3','Digit 4','Digit 5','Digit 1 COM','Digit 1 END','Digit 2 COM','Digit 2 END','Digit 3 COM','Digit 3 END','Digit 4 COM','Digit 4 END','Digit 5 COM','Digit 5 END')
pause(.5)
end
pause(.5)
end
pause(.5)
end
pause(.5)
end
pause(.5)
end
%% Velocity 
Rot_speed = 2000/60; %(Rot/sec)
r = 0.5;
C = 2*pi*r;
Speed = Rot_speed*C;
t = C/Rot_speed;
%v=dx/dt

